#include "qbot_master.h"

SerialPort serial_port("/dev/ydlidar", 921600);
QbotMaster qbot(&serial_port);

int main(void)
{
    qbot.init();
    for(int i=0;i<1000;i++)
    {
        qbot.send_message(ID_INIT_ODOM);
        boost::thread::sleep(boost::get_system_time()+boost::posix_time::milliseconds(5));
        qbot.send_message(ID_RESET_WHEELS);
        boost::thread::sleep(boost::get_system_time()+boost::posix_time::milliseconds(5));
    }
    for(;;);
    return 0;
}
